"""
navigation2.launch.py

This launch file is used to start the ROS 2 navigation stack for the robot.
It includes the following components:
- Navigation2 bringup launch file
- RViz2 visualization tool

The launch file configures:
- Simulation time usage
- Navigation parameters file
- Map file location
- RViz configuration
"""

import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # Get package share directories
    # Get paths to required resources
    wrobot_navigation2_path = get_package_share_directory("wrobot_navigation2")  # Path to this package
    nav2_bringup_path = get_package_share_directory("nav2_bringup")  # Path to nav2_bringup package
    rviz_config_path = os.path.join(nav2_bringup_path,"rviz","nav2_default_view.rviz")  # RViz config file path
    
    # Configure launch parameters
    # Define launch configurations
    use_sim_time = launch.substitutions.LaunchConfiguration("use_sim_time",default = "true")  # Use simulation time
    nav2_param_path = launch.substitutions.LaunchConfiguration("params_file",default = os.path.join(wrobot_navigation2_path,"config","nav2_params.yaml"))  # Navigation parameters file
    map_yaml_path = launch.substitutions.LaunchConfiguration("map",default = os.path.join(wrobot_navigation2_path,"maps","room.yaml"))  # Map file path

    return launch.LaunchDescription([
        # Declare launch arguments that can be overridden from the command line
        # Declare launch arguments with default values
        launch.actions.DeclareLaunchArgument(
            "use_sim_time",
            default_value = use_sim_time),
            
        launch.actions.DeclareLaunchArgument(
            "map",
            default_value = map_yaml_path),
            
        launch.actions.DeclareLaunchArgument(
            "params_file",
            default_value = nav2_param_path),
            
        # Include the main navigation2 bringup launch file
        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_bringup_path,"/launch","/bringup_launch.py"]),
            launch_arguments = {
                "map": map_yaml_path,
                "use_sim_time":use_sim_time,
                "params_file": nav2_param_path}.items(),
            ),
            
        # Launch RViz2 for visualization
        launch_ros.actions.Node(
            package = "rviz2",
            executable = "rviz2",
            name = "rviz2",
            arguments = ['-d',rviz_config_path],
            parameters = [{"use_sim_time": use_sim_time}],
            output = "screen",
            )
    ])
